#ifndef _BOOST_POLY_H_
#define _BOOST_POLY_H_

#include <boost/math/tools/polynomial.hpp>
#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/tools/assert.hpp>
using namespace boost::math;
using namespace boost::math::tools; // for polynomial
using boost::lexical_cast;

#include <iostream>
#include <stdexcept>
#include <cmath>
#include <string>
#include <utility>

// kfr
#include <kfr/base/filter.hpp>
#include <kfr/base.hpp>
#include <kfr/dsp.hpp>
//#ifdef _KFR_PLOT
#include <kfr/io.hpp> // plot_save
//#endif
using namespace kfr;

std::vector< std::vector<fbase> > sos2tf(std::vector< biquad_params<fbase> >& bqs)
{
  std::vector< polynomial<fbase> > out_poly(2);

  out_poly[0] = polynomial<fbase>({bqs[0].b0, bqs[0].b1, bqs[0].b2});
  out_poly[1] = polynomial<fbase>({bqs[0].a0, bqs[0].a1, bqs[0].a2});
  
  for (size_t i = 1; i < bqs.size(); i++) {
    out_poly[0] *= polynomial<fbase>({bqs[i].b0, bqs[i].b1, bqs[i].b2});
    out_poly[1] *= polynomial<fbase>({bqs[i].a0, bqs[i].a1, bqs[i].a2});
  }

  std::vector< std::vector<fbase> > out(2);
  out[0] = out_poly[0].data();
  out[1] = out_poly[1].data();
  
  return out;
}

void print_sos( std::vector< biquad_params<fbase> >& bqs)
{
  for(size_t i = 0; i < bqs.size(); i++) {
    printf("  bqs = (%+2.8f, %+2.8f, %+2.8f, %+2.8f, %+2.8f, %+2.8f)\n", bqs[i].b0, bqs[i].b1, bqs[i].b2, bqs[i].a0, bqs[i].a1, bqs[i].a2);
  }
}

std::vector< std::vector<fbase> > sos2tf1(std::vector< biquad_params<fbase> >& bqs)
{
  std::vector< polynomial<fbase> > out_poly(2);

  out_poly[0] = polynomial<fbase>({bqs[0].b0, bqs[0].b1, bqs[0].b2});
  out_poly[1] = polynomial<fbase>({bqs[0].a0, bqs[0].a1, bqs[0].a2});
  
  for (size_t i = 1; i < bqs.size(); i++) {
    auto &in_bqs = bqs[i];
    std::vector<fbase> b({in_bqs.b0, in_bqs.b1, in_bqs.b2});
    std::vector<fbase> a({in_bqs.a0, in_bqs.a1, in_bqs.a2});
    polynomial<fbase> b_poly = polynomial<fbase>(b);
    polynomial<fbase> a_poly = polynomial<fbase>(a);
    out_poly[0] *= b_poly;
    out_poly[1] *= a_poly;
  }

  std::vector< std::vector<fbase> > out(2);
  out[0] = out_poly[0].data();
  out[1] = out_poly[1].data();
  
  return out;
}

#endif
